// #include "../../include/3d_library/rvo_node.h"
#include "../rvo_node.h"


int IRIS_NUM = 0;
// float Initial_pos_x = 0.0;
// float Initial_pos_y = 0.0;



uint64_t seq = 0;       // 定义全局变量 seq 用于记录消息序列号
std::string agent_name; // 定义全局变量 agent_name 用于存储智能体名称
int num_publishers=1;

struct Position {
    float x;
    float y;
    float z;
    // 构造函数
    Position(float x, float y, float z) : x(x), y(y), z(z) {}
};
std::vector<Position> positions;

bool flag=true;

int main(int argc, char **argv)
{
    // 初始化ROS节点
    std::string node_name = "rvo_node_"+std::to_string(IRIS_NUM);
    ros::init(argc, argv, node_name);
    ros::NodeHandle n;

    // 广播rvo_vel话题
    //rvo_node_pub = n.advertise<gazebo_msgs::WorldState>("rvo_vel", 1000);

    ros::Subscriber sub_0 = n.subscribe("/gazebo/model_states", 1, get_initial_pos);
    //发布到xtdrone
     

    // 初始化每个发布者并添加到向量中

    std::string topic_name = "/xtdrone/iris_" + std::to_string(IRIS_NUM) + "/cmd_vel_flu";
    ros::Publisher pub = n.advertise<geometry_msgs::Twist>(topic_name, 1000);
    single_cmd_vel_flu_pub.push_back(pub);
    

    // 设置循环频率为50Hz
    ros::Rate loop_rate(50);

    // 处理命令行参数
    if ((argc > 1) && (argc % 3 == 1))
    {
        int num_init_point = argc - 1;
        for (int i = 1; i < num_init_point + 1; i = i + 3)
        {
            geometry_msgs::Point point;
            point.x = atof(argv[i]);
            point.y = atof(argv[i + 1]);
            point.z = atof(argv[i + 2]);
            rvo_goals.push_back(point);
        }
    }
    else
    {
        ROS_INFO("No input, Using default position 0 1 0 2 ....0 10 ");
    }

    // 获取参数服务器中的参数
    double neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed, goal_threshold;
    n.param<double>("neighborDist", neighborDist, 4);
    n.param<double>("maxNeighbors", maxNeighbors, 10);
    n.param<double>("timeHorizon", timeHorizon, 10);
    n.param<double>("timeHorizonObst", timeHorizonObst, 5);
    n.param<double>("radius", radius, 0.3);
    n.param<double>("maxSpeed", maxSpeed, 0.2);
    n.param<double>("goal_threshold", goal_threshold, 0.01);
    n.param<std::string>("agent_name", agent_name, "iris_");


    while(flag){
        ros::spinOnce();   // 处理所有挂起的回调函数
        loop_rate.sleep(); // 休眠以使循环以指定频率运行
    }
    // 创建RVOPlanner对象并进行初始化
    rvo = new RVO::RVOPlanner("gazebo");

    rvo->goal_threshold = goal_threshold;
    rvo->setupScenario(neighborDist, maxNeighbors, timeHorizon, timeHorizonObst, radius, maxSpeed);
    // // 目标点为空才起效果
    rvo_goals_init();

    // 订阅/gazebo/model_states话题
    ros::Subscriber sub = n.subscribe("/gazebo/model_states", 100, rvo_velCallback);
    // 广告set_rvo_goals服务
    std::string rvo_goals_topic_name = "set_rvo_goals_"+std::to_string(IRIS_NUM);
    ros::ServiceServer service = n.advertiseService(rvo_goals_topic_name,  set_goals);

    

    std::cout << "Configure completely" << std::endl;

    // 主循环，持续运行节点
    while (ros::ok())
    {
        ros::spinOnce();   // 处理所有挂起的回调函数
        loop_rate.sleep(); // 休眠以使循环以指定频率运行
    }
}

void get_initial_pos(const gazebo_msgs::ModelStates::ConstPtr &model_msg)
{
    auto models_name = model_msg->name; // 获取所有模型的名称
    int num = models_name.size();       // 获取模型数量
    int count = 0;                      // 计数器，用于跟踪智能体数量

    // 遍历所有模型
    for (int i = 0; i < num; i++)
    {
        std::string full_agent_name = agent_name + std::to_string(i); // 构建完整的智能体名称

        // 查找当前智能体名称在模型名称列表中的位置
        auto iter_agent = std::find(models_name.begin(), models_name.end(), full_agent_name);
        int agent_index = iter_agent - models_name.begin(); // 获取智能体在模型名称列表中的索引

        // 如果找到了智能体
        if (iter_agent != models_name.end())
        {
            // 获取智能体的位置和速度

            float obs_x = model_msg->pose[agent_index].position.x;
            float obs_y = model_msg->pose[agent_index].position.y;
            float obs_z = model_msg->pose[agent_index].position.z;

            // std::cout << "@@@@@@@@@@position" << count << ": " << obs_x << " " << obs_y << " " << obs_z << " "  << std::endl;

            positions.emplace_back(obs_x,obs_y,obs_z);

            count++; // 增加智能体计数器
        }
    }
    flag=false;
}

// 用于处理 SetGoals 服务请求。该函数根据请求中指定的模型类型（"default", "circle", "circle_spin", "random"）设置目标点，并返回目标点的数量。
bool set_goals(rvo_ros::SetGoals::Request &req, rvo_ros::SetGoals::Response &res)
{
    // 处理 "default" 模型类型
    if (req.model == "default")
    {
        // 设置当前运动模型为请求中的模型类型
        motion_model = req.model;

        // 清空现有的目标点列表
        if (!rvo_goals.empty())
            rvo_goals.clear();

        // 将请求中的坐标点添加到目标点列表中
        for (const auto &coordinate : req.coordinates)
        {
            rvo_goals.push_back(coordinate);
        }

        // 设置响应中的目标点数量
        res.num_goal = rvo_goals.size();

        return true;
    }

    // 处理 "circle" 模型类型
    if (req.model == "circle")
    {
        // 设置当前运动模型为请求中的模型类型
        motion_model = req.model;

        // 清空现有的目标点列表
        if (!rvo_goals.empty())
            rvo_goals.clear();

        // 获取圆心坐标和半径
        float circle_point_x = req.coordinates[0].x;
        float circle_point_y = req.coordinates[0].y;
        float circle_point_z = req.coordinates[0].z;
        float radius = req.coordinates[1].x;

        // 获取圆的平面和排列标志
        int circle_plane = req.coordinates[1].y;
        int flag = req.coordinates[1].z;

        // 定义圆周率和角度步长
        float pi = 3.14;
        float theta_step = 2 * pi / num_agent;
        float theta = 0;

        // 按角度步长生成圆周上的点
        while (theta < 2 * pi)
        {
            geometry_msgs::Point point;

            // 根据不同的平面生成点的坐标
            if (circle_plane == 0) // xy 平面
            {
                point.x = circle_point_x + cos(theta) * radius;
                point.y = circle_point_y + sin(theta) * radius;
                point.z = circle_point_z;
            }
            else if (circle_plane == 1) // yz 平面
            {
                point.x = circle_point_x;
                point.y = circle_point_y + cos(theta) * radius;
                point.z = circle_point_z + sin(theta) * radius;
            }
            else if (circle_plane == 2) // xz 平面
            {
                point.x = circle_point_x + cos(theta) * radius;
                point.y = circle_point_y;
                point.z = circle_point_z + sin(theta) * radius;
            }

            // 将生成的点添加到目标点列表中
            rvo_goals.push_back(point);
            theta += theta_step;
        }

        // 设置响应中的目标点数量
        res.num_goal = rvo_goals.size();

        return true;
    }

    // 处理 "circle_spin" 模型类型
    if (req.model == "circle_spin")
    {
        // 设置当前运动模型为请求中的模型类型
        motion_model = req.model;

        // 清空现有的目标点列表
        if (!rvo_goals.empty())
            rvo_goals.clear();

        // 获取圆心坐标和半径
        float circle_point_x = req.coordinates[0].x;
        float circle_point_y = req.coordinates[0].y;
        float circle_point_z = req.coordinates[0].z;
        float radius = req.coordinates[1].x;

        // 定义圆周率和角度步长
        float pi = 3.1415926;
        float theta_step = 2 * pi / num_agent;
        float theta = 0;

        // 按角度步长生成圆周上的点
        while (theta < 2 * pi)
        {
            geometry_msgs::Point point;
            point.x = circle_point_x + cos(theta) * radius;
            point.y = circle_point_y + sin(theta) * radius;
            point.z = circle_point_z;

            // 将生成的点添加到目标点列表中
            rvo_goals.push_back(point);

            // 更新角度
            theta = theta + theta_step;
        }

        // 设置响应中的目标点数量
        res.num_goal = rvo_goals.size();

        return true;
    }

    // 处理 "random" 模型类型
    if (req.model == "random")
    {
        // 设置当前运动模型为请求中的模型类型
        motion_model = req.model;

        // 检查输入坐标点是否足够
        if (req.coordinates.size() < 2)
        {
            ROS_ERROR("too less input");
            return false;
        }
        else
        {
            // 获取随机生成目标点的坐标范围
            limit_goal[0] = req.coordinates[0].x; // x_min
            limit_goal[1] = req.coordinates[1].x; // x_max
            limit_goal[2] = req.coordinates[0].y; // y_min
            limit_goal[3] = req.coordinates[1].y; // y_max
            limit_goal[4] = req.coordinates[0].z; // z_min
            limit_goal[5] = req.coordinates[1].z; // z_max

            // 设置响应中的目标点数量为智能体数量
            res.num_goal = num_agent;

            // 调用生成随机目标点的函数
            rvo->randomOnceGoal(limit_goal);
            return true;
        }
    }

    // 如果模型类型不匹配，返回错误信息
    std::cout << "The specific model is wrong" << std::endl;
    return false;
}

// 用于初始化 rvo_goals 列表。如果 rvo_goals 列表为空，该函数会将一系列预定义的目标点添加到列表中。
// 用于在系统启动或重置时初始化目标点列表，确保 rvo_goals 列表中有一组预定义的目标点。
void rvo_goals_init()
{
    // 检查目标点列表是否为空
    if (rvo_goals.empty())
    {
        //如果目标点列表为空，初始化目标点列表
        for (int i = 0; i < num_max; i++)
        {
            // 创建一个新的几何消息点
            geometry_msgs::Point point;

            // 设置点的坐标
            if(i == IRIS_NUM){
                point.x = positions.at(IRIS_NUM).x;
                point.y = positions.at(IRIS_NUM).y;
                // point.x = Initial_pos_x;
                // point.y = Initial_pos_y;
                point.z = 2.0; 
            }
            else{
                // point.x = positions.at(i).x;
                // point.y = positions.at(i).y;
                point.x = 4;
                point.y = 0;
                point.z = 2.0;      // z 坐标设置为 0.0
            }
            // 将点添加到目标点列表中
            rvo_goals.push_back(point);
        }
    }
}

// 用于处理从 gazebo_msgs::ModelStates 话题接收到的消息。该函数会更新智能体的状态，设置目标，计算新的速度，并发布更新后的智能体状态。
// 用于在接收到新的模型状态消息时更新智能体的状态、计算新的速度并发布更新后的智能体状态。
void rvo_velCallback(const gazebo_msgs::ModelStates::ConstPtr &sub_msg)
{
    // 增加序列号
    seq++;
    int count_vel = 0;

    // 更新智能体的状态
    int uav_num = 0;
    rvo->updateState_gazebo(sub_msg, agent_name, uav_num);

    // 根据不同的运动模型设置目标
    if (motion_model == "default")
        rvo->setGoal(rvo_goals);
    else if (motion_model == "random")
        rvo->randGoal(limit_goal, "default");
    else if (motion_model == "circle")
        rvo->setGoal(rvo_goals);
    else if (motion_model == "circle_spin")
        rvo->setGoal(rvo_goals);

    // if(rvo_goals.empty()){
    //     std::cout << "The  rvo_goals is empty!"<< std::endl;
    //     return;
    //     }

    // 设置智能体的初始状态和偏好速度
    rvo->setInitial(); //是为了获得一个bool值IfInitial：是否有goals和agents
    rvo->setPreferredVelocities();//设置所有sim->getNumAgents()的偏好速度

    // 检查是否所有智能体都到达目标，没啥用，不是circle_spin模式
    bool arrive_flag = rvo->arrived();

    // 如果运动模型是 "circle_spin"，并且所有智能体都到达目标，则循环移动目标点
    if (motion_model == "circle_spin")
    {
        if (arrive_flag == true)
        {
            geometry_msgs::Point temp_first = rvo_goals[0];

            for (int i = 0; i < rvo_goals.size() - 1; i++)
            {
                rvo_goals[i] = rvo_goals[i + 1];
            }

            rvo_goals[rvo_goals.size() - 1] = temp_first;
        }
    }

    // 计算新的速度
    // 其实只算自己的新速度即可，也就是new_velocities[0]
    std::vector<RVO::Vector3 *> new_velocities = rvo->step();

    // 获取模型的名称列表
    auto models_name = sub_msg->name;
    int total_num = models_name.size();

    // 设置消息头
    std_msgs::Header header;
    header.seq = seq;
    header.stamp = ros::Time::now();
    header.frame_id = "/world";

    msg_pub.header = header;
    msg_pub.name.clear();
    msg_pub.pose.clear();
    msg_pub.twist.clear();

    //my_msg.twist.clear();


    num_agent = new_velocities.size();
    //std::cout<<num_agent;
    
    //ROS_INFO(""+copy_num_agent);

    // 如果智能体数量发生变化，输出智能体数量
    if (num_agent != copy_num_agent)
    {
        std::cout << "The num of agents is " + std::to_string(num_agent) << std::endl;
        copy_num_agent = num_agent;
    }

    //遍历发布更新后的速度信息
    geometry_msgs::Twist new_vel;

    std::string full_agent_name = agent_name + std::to_string(IRIS_NUM);

    // 查找当前智能体名称在模型名称列表中的位置
    auto iter_agent = std::find(models_name.begin(), models_name.end(), full_agent_name);
    int iter_index = iter_agent - models_name.begin();

    // 如果找到了对应的智能体名称，更新速度
    if (iter_agent != models_name.end())
    {
        float x = new_velocities[IRIS_NUM]->x();
        float y = new_velocities[IRIS_NUM]->y();
        float z = new_velocities[IRIS_NUM]->z();

        new_vel.linear.x = x;
        new_vel.linear.y = y;
        new_vel.linear.z = z;
        
        //my_msg.twist.push_back(new_vel);
        single_cmd_vel_flu_pub[0].publish(new_vel);

    }

}
